import math

import cv2
import numpy as np
from monocular import Pose_estimation
from p3p import P3P

a = math.acos(230/234) / math.pi * 180
b = math.acos(230/232) / math.pi * 180
c = math.acos(230/231) / math.pi * 180
d = math.acos(230/231.5) / math.pi * 180
e = math.acos(230/236) / math.pi * 180

pose_estimation = Pose_estimation(camera_matrix=np.array([[3.888070611112474e+02, 0, 3.106686625843250e+02],
                                                          [0, 3.909007083701075e+02, 2.583920557919889e+02],
                                                          [0, 0, 1]]),
                                  dist_coeffs=np.array([0.1642, -0.165, 0, 0, 0]))


def on_EVENT_LBUTTONDOWN(event, x, y, flags, param):
    if event == cv2.EVENT_LBUTTONDOWN:
        pitch, yaw = pose_estimation.pixelToangle(x, y)
        '''
        pitch, yaw = pose_estimation.P4p(pixel_points=np.array([[249, 456],
                                                                [255, 456],
                                                                [242, 313],
                                                                [254, 313]], dtype=np.double),
                                         world_points=np.array([[0, 0, 0],
                                                                [14, 0, 0],
                                                                [0, 100, 0],
                                                                [14, 100, 0]], dtype=np.double))
        '''
        xy = "%d,%d" % (x, y)
        cv2.circle(img, (x, y), 1, (255, 0, 0), thickness=-1)
        cv2.putText(img, xy, (x, y), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (0, 0, 0), thickness=1)
        cv2.putText(img, str(pitch), (x, y - 15), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (0, 0, 0), thickness=1)
        cv2.putText(img, str(yaw), (x, y - 30), cv2.FONT_HERSHEY_PLAIN,
                    1.0, (0, 0, 0), thickness=1)
        cv2.imshow("image1", img)



cap = cv2.VideoCapture(1)
cv2.namedWindow("image")
cv2.setMouseCallback("image", on_EVENT_LBUTTONDOWN)
while (1):
    ret, img = cap.read()
    cv2.imshow("image", img)

    if cv2.waitKey(1) & 0xFF == 27:
        break
cv2.destroyAllWindows()
